/**
* @file         witdatapkg.cpp
* @author       David Hu (hmd_hubei_cn@163.com)
* @brief         
* @version      0.1
* @date         2024.10.16
* @note          
* @copyright    Copyright (c) 2022 DAVID HU All rights reserved. Licensed under the MIT License (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License in the file LICENSE
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
**/

#include "witdatapkg.h"
#include <stdio.h>

namespace wit_private_msg {

WitDataPkg::WitDataPkg() {
  unpack_data_buffer_len_ = 0;

  unpack_handle_.state = WIT_HEADER;
  unpack_handle_.package_len = 0;
  unpack_handle_.package_index = 0;
  unpack_handle_.data_id = 0;
  unpack_handle_.data_in_buff = &unpack_buffer_[0];
  unpack_handle_.data_in_len = 0;
  unpack_handle_.data_out_buff = NULL;
  unpack_handle_.data_out_len = &unpack_data_buffer_len_;

  imu_acc_.acc_x = 0;
  imu_acc_.acc_y = 0;
  imu_acc_.acc_z = 0;

  imu_angularvel_.angularvel_x = 0;
  imu_angularvel_.angularvel_y = 0;
  imu_angularvel_.angularvel_z = 0;

  imu_angular_.roll_x = 0;
  imu_angular_.pitch_y = 0;
  imu_angular_.yaw_z = 0;

  gnss_locations_.current_pos_lng = 0;
  gnss_locations_.current_pos_lat = 0;
  gnss_locations_.actual_speed = 0;
  gnss_locations_.actual_altitude = 0;

  is_imu_ready_ = false;
  is_gnss_locations_ready_ = false;
}

WitDataPkg::~WitDataPkg() {

}

void WitDataPkg::RecvDataCallback(const char* data, uint16_t len) {
  for (int i = 0; i < len; i++) {
    ParseCmd((uint8_t)data[i]);
  }
}

WitImuAccelerometerType WitDataPkg::GetImuAccelerometerData(void) const {
  return imu_acc_;
}

WitImuAngularvelType WitDataPkg::GetImuAngularvelData(void) const {
  return imu_angularvel_;
}

WitImuAngularType WitDataPkg::GetImuAngularData(void) const {
  return imu_angular_;
}

WitGnssLocationType WitDataPkg::GetGnssLocationData(void) const {
  return gnss_locations_;
}

bool WitDataPkg::IsImuDataReady(void) {
  if (is_imu_ready_) {
    is_imu_ready_ = false;
    return true;
  } else {
    return false;
  }
}

bool WitDataPkg::IsGnssDataReady(void) {
  if (is_gnss_locations_ready_) {
    is_gnss_locations_ready_ = false;
    return true;
  } else {
    return false;
  }
}

void WitDataPkg::ParseCmd(uint8_t data) {
  if (wit_unpack(data, &unpack_handle_)) {
    uint8_t id = GetPackId();
    switch (id) {
      case WIT_PKG_ID_ACC_OUT: {
        uint8_t* pdata = GetPackDataBuff();
        uint16_t datalen = GetPackDataLen();
        if ((pdata != NULL) && (datalen == sizeof(imu_acc_msg_t))) {
          imu_acc_msg_t msg = *(imu_acc_msg_t*)pdata;
          float acc_x = (float)(msg.acc_x) * 16 / 32768;
          float acc_y = (float)(msg.acc_y) * 16 / 32768;
          float acc_z = (float)(msg.acc_z) * 16 / 32768;
          // printf("ACC_X:%f g,ACC_Y:%f g,ACC_Z:%f g\n", acc_x, acc_y, acc_z);
          // printf("TEMP:%d C\n", msg.temper / 100);
          // printf("------------\n");
          imu_acc_.acc_x = acc_x;
          imu_acc_.acc_y = acc_y;
          imu_acc_.acc_z = acc_z;
          is_imu_ready_ = true;
        }
        break;
      }
      case WIT_PKG_ID_ANGULARVEL_OUT: {
        uint8_t* pdata = GetPackDataBuff();
        uint16_t datalen = GetPackDataLen();
        if ((pdata != NULL) && (datalen == sizeof(imu_angularvel_msg_t))) {
          imu_angularvel_msg_t msg = *(imu_angularvel_msg_t*)pdata;
          float w_x = (float)msg.w_x * 2000 / 32768;
          float w_y = (float)msg.w_y * 2000 / 32768;
          float w_z = (float)msg.w_z * 2000 / 32768;
          // printf("W_X:%f deg/s\n", w_x);
          // printf("W_Y:%f deg/s\n", w_y);
          // printf("W_Z:%f deg/s\n", w_z);
          // printf("------------\n");
          imu_angularvel_.angularvel_x = w_x;
          imu_angularvel_.angularvel_y = w_y;
          imu_angularvel_.angularvel_z = w_z;
          is_imu_ready_ = true;
        }
        break;
      }
      case WIT_PKG_ID_ANGULAR_OUT: {
        uint8_t* pdata = GetPackDataBuff();
        uint16_t datalen = GetPackDataLen();
        if ((pdata != NULL) && (datalen == sizeof(imu_angular_msg_t))) {
          imu_angular_msg_t msg = *(imu_angular_msg_t*)pdata;
          float roll = (float)msg.roll * 180 / 32768;
          float pitch = (float)msg.pitch * 180 / 32768;
          float yaw = (float)msg.yaw * 180 / 32768;
          // printf("Roll:%f deg\n", roll);
          // printf("Pitch:%f deg\n", pitch);
          // printf("Yaw:%f deg\n", yaw);
          // printf("VersionNum:v%d.%d\n", msg.vh, msg.vl);
          // printf("------------\n");
          imu_angular_.roll_x = roll;
          imu_angular_.pitch_y = pitch;
          imu_angular_.yaw_z = yaw;
          is_imu_ready_ = true;
        }
        break;
      }
      case WIT_PKG_ID_LON_LAT_OUT: {
        uint8_t *pdata = GetPackDataBuff();
        uint16_t datalen = GetPackDataLen();
        if ((pdata != NULL) && (datalen == sizeof(gps_longitude_latitude_msg_t))) {
          gps_longitude_latitude_msg_t lon_lat = *(gps_longitude_latitude_msg_t *)pdata;
          int lng_ = lon_lat.lonval;
          int lat_ = lon_lat.latval;
          float current_pos_lng = lng_ / 10000000 + (lng_ % 10000000) / 100000.0 / 60.0;
          float current_pos_lat = lat_ / 10000000 + (lat_ % 10000000) / 100000.0 / 60.0;
          // printf("current lng: %f, current lat: %f \n", current_pos_lng, current_pos_lat);
          // printf("------------\n");
          gnss_locations_.current_pos_lng = current_pos_lng;
          gnss_locations_.current_pos_lat = current_pos_lat;
          is_gnss_locations_ready_ = true;
        }
        break;
      }
      case WIT_PKG_ID_GPS_DATA_OUT: {
        uint8_t *pdata = GetPackDataBuff();
        uint16_t datalen = GetPackDataLen();
        if ((pdata != NULL) && (datalen == sizeof(gps_base_data_msg_t))) {
          gps_base_data_msg_t gps_data = *(gps_base_data_msg_t *)pdata;
          int speed_ = (gps_data.gps_speed[3] << 24) | (gps_data.gps_speed[2] << 16) |
                  (gps_data.gps_speed[1] << 8) | gps_data.gps_speed[0];
          int actual_speed = (int)(speed_ / 360);  // 计算m/s速度    unit : 0.1m/s
          int height = (gps_data.gps_height_msb << 8) | gps_data.gps_height_lsb; // unit: m
          // printf("act speed: %d, height: %d\n", actual_speed, height);
          // printf("------------\n");
          gnss_locations_.actual_speed = actual_speed;
          gnss_locations_.actual_altitude = height;
          is_gnss_locations_ready_ = true;
        }
        break;
      }
    }
  }
}

uint8_t WitDataPkg::GetPackId(void) {
  return unpack_handle_.data_id;
}

uint8_t* WitDataPkg::GetPackDataBuff(void) {
  return unpack_handle_.data_out_buff;
}

uint32_t WitDataPkg::GetPackDataLen(void) {
  return unpack_data_buffer_len_;
}


}  // namespace wit_private_msg
/********************* (C) COPYRIGHT DAVID HU *******END OF FILE ********/
